#!/bin/bash
# ============================================================
# Script Name:    baseBringup.sh
# Description:    启动AGV系统基础核心服务
#                 包括固件服务、参数服务器、FTP服务和AGV核心服务
# Author:         haibo
# Created Date:   2025-07-24
# Version:        1.0
# Modified Date:  2025-09-09
# Modified By:    haibo
# Modification:   初始版本
# Usage:          ./baseBringup.sh [日志文件路径]
# Notes:          1. 此脚本是AGV系统的基础服务启动入口
#                 2. 依赖common.sh公共函数库
#                 3. 服务启动顺序按依赖关系排列
# ============================================================
source ${HOME}/lgimRobot/bin/common "$@"

start_params_server() {
    launch_node "参数服务器" ros2 run shared_parameters param_server
    sleep 1

    PARAMS=$(load_params)
    load_params_to_map "$PARAMS"

    dump_param_map
    monitor_params_server
    error_code=$?

    if [ $error_code -ne 0 ]; then
        log "ERROR" "监控参数服务器失败, 退出启动"
        exit 1
    fi
}

main() {
    start_params_server
    local result=$?
    if [ $result -gt 0 ]; then
        return $result
    fi
    
    launch_node "固件服务" ros2 run firmware firmware
    launch_node "FTP服务1" ros2 run agv_ftp_log log_file_config
    launch_node "FTP服务2" ros2 run agv_ftp_log agv_param_transform
    launch_node "record" ros2 run record record_node
    launch_node "twist_mux" ros2 run twist_mux twist_mux --ros-args \
        --remap cmd_vel_out:=/cmd_vel \
        --params-file $(ros2 pkg prefix agv_bringup)/share/agv_bringup/config/default_twist_mux_locks.yaml \
        --params-file $(ros2 pkg prefix agv_bringup)/share/agv_bringup/config/default_twist_mux_topics.yaml
    launch_node "tcp_bridge" ros2 run agv_tcp_bridge tcp_bridge

    launch_node "kacanopen" ros2 run kacanopen kacanopen_apps_kacanopen_new --ros-args -r __node:=kacanopen
    launch_node "code_read_camera" ros2 run code_read_camera code_read_camera_node
    launch_node "tf_laser" ros2 run tf_laser laser_transform_publisher     
    launch_node "modbus" ros2 run modbus master_node
    launch_node "io_module" ros2 run io_module agv_set_io
    launch_node "indicators" ros2 run agv_indicators agv_indicators
    launch_node "agv_teleop" ros2 run agv_teleop agv_teleop_HD1000
    launch_node "agv_dispatch" ros2 run agv_dispatch_comm agv_dispatch_comm_node --ros-args -r __node:=agv_dispatch
    launch_node "controller_manager" ros2 run controller_manager ros2_control_node
    launch_node "agv_obstacle_avoidance" ros2 run agv_obstacle_avoidance agv_obstacle_avoidance
    launch_node "monitor" ros2 run state_monitor monitor
    launch_node "agv_navigate" ros2 run agv_navigate agv_navigate --ros-args --remap /cmd_vel:=/twist_mux/nav_vel 

    echo "$PARAMS"
    return 0
}

main